#!/usr/bin/env python
# -*- coding: utf-8 -*-

#combination of cf and other information

import rospy
from std_msgs.msg import String
from pycrazyswarm import Crazyswarm


TAKEOFF_DURATION = 2.5
HOVER_DURATION = 5.0
formation=String()
formation_ex=String()
formation_ex.data='NULL'

def formationCallback(msg):
    formation=msg
    rospy.loginfo("The Instruction Received : %s", formation.data)


def command_subscriber():
	# ROS节点初始化
    rospy.init_node('subscribe_string')
	# 创建一个Subscriber，订阅名为/std_msgs/string的topic，注册回调函数poseCallback
    rospy.Subscriber("/std_msgs/string", String, formationCallback)
	# 循环等待回调函数
    rospy.spin()

def UP():
    swarm = Crazyswarm()
    timeHelper = swarm.timeHelper
    cf = swarm.allcfs.crazyflies[0]

    cf.takeoff(targetHeight=1.0, duration=TAKEOFF_DURATION)
    timeHelper.sleep(TAKEOFF_DURATION + HOVER_DURATION)

def DOWN():
    cf.land(targetHeight=0.04, duration=2.5)
    timeHelper.sleep(TAKEOFF_DURATION)

if __name__ == '__main__':
	while not rospy.is_shutdown():
		command_subscriber()
		if formation_ex.data!=formation.data
			if formation.data == 'UP':
				UP()

			elif formation.data == 'DOWN':
				DOWN()

			formation_ex=formation
	end

